#include "graph.h"
#include "node.h"
#include "connection.h"
#include "priorityheap.h"
#include "priorityqueue.h"
#include "noderecord.h"
#include "astarsearch.h"
#include "astarpathfinder.h"


AStarPathfinder::AStarPathfinder(int argc, char *argv[])
	: QCoreApplication(argc, argv), testGraph_(new Graph(64))
{
	buildGraph();
	testBinaryHeap();
	//testNodeRecords();
	testAStar();
}

AStarPathfinder::~AStarPathfinder()
{
	delete testGraph_;
}



void AStarPathfinder::buildGraph()
{
	int width = 8;
	int height = 8;

	//nodes for 8x8 test field
	for(int i=0; i<height; ++i)
	{
		for(int j=0; j<width; j++)
		{
			Node* newNode = new Node(i*width + j);
			newNode->setPosition(j, i);
			testGraph_->addNode(newNode);
		}
	}

	//connections
	for(int row=0;row<height;++row)
	{
		for(int col=0;col<width;++col)
		{
			int currentField = row*width + col;
			
			int leftNeighbor = currentField-1;
			int rightNeighbor = currentField+1;
			int topNeighbor = currentField+width;
			int lowerNeighbor = currentField-width;

			int topRightNeighbor = currentField+width+1;
			int topLeftNeighbor = currentField+width-1;
			int lowerRightNeighbor = currentField-width+1;
			int lowerLeftNeighbor = currentField-width-1;

			//checking bounds for the four potential neighbors
			if(leftNeighbor >= (width*row)) //left neighbor
			{
				Connection* nodeConnection = new Connection(currentField, leftNeighbor);
				testGraph_->addConnection(nodeConnection);
			}	

			if(rightNeighbor < width*row+width) //right neighbor
			{
				Connection* nodeConnection = new Connection(currentField, rightNeighbor);
				testGraph_->addConnection(nodeConnection);
			}

			if(topNeighbor < width*height) //upper neighbor
			{
				Connection* nodeConnection = new Connection(currentField, topNeighbor);
				testGraph_->addConnection(nodeConnection);
			}

			if(lowerNeighbor >= 0)//lower neighbor
			{
				Connection* nodeConnection = new Connection(currentField, lowerNeighbor);
				testGraph_->addConnection(nodeConnection);
			}


			if(topLeftNeighbor >= (width*(row+1)) && topLeftNeighbor < width*height)
			{
				Connection* nodeConnection = new Connection(currentField, topLeftNeighbor);
				testGraph_->addConnection(nodeConnection);
			}

			if(topRightNeighbor < width*(row+1)+width && topRightNeighbor < width*height)
			{
				Connection* nodeConnection = new Connection(currentField, topRightNeighbor);
				testGraph_->addConnection(nodeConnection);
			}

			if(lowerLeftNeighbor >= width*(row-1) && lowerLeftNeighbor >= 0)
			{
				Connection* nodeConnection = new Connection(currentField, lowerLeftNeighbor);
				testGraph_->addConnection(nodeConnection);
			}

			if(lowerRightNeighbor < width*(row-1)+width && lowerRightNeighbor >= 0)
			{
				Connection* nodeConnection = new Connection(currentField, lowerRightNeighbor);
				testGraph_->addConnection(nodeConnection);
			}
		}
	}

	//costs
	for(size_t i=0;i<testGraph_->getNodeAmount();++i)
	{
		//set all costs to 1
		Graph::AdjacentConnections nodeConnections = testGraph_->getConnections(i);
		
		Graph::AdjacentConnections::const_iterator connectionsIt;

		for(connectionsIt=nodeConnections.begin();connectionsIt!=nodeConnections.end();++connectionsIt)
		{
			(*connectionsIt)->setCost(1);
		}
	}
}

void AStarPathfinder::testBinaryHeap()
{
	std::vector<double> costs;
	costs.push_back(2.0);
	costs.push_back(2.0);
	costs.push_back(3.0);
	costs.push_back(1.0);
	IndexedPriorityQueue<double> priorityQueue(costs, costs.size());

	
	priorityQueue.insert(0);
	priorityQueue.insert(1);
	priorityQueue.insert(2);
	priorityQueue.insert(3);
}

void AStarPathfinder::testNodeRecords()
{
	PriorityHeap<NodeRecord>* priorityQueue = new PriorityHeap<NodeRecord>(10);

	NodeRecord rec1;
	NodeRecord rec2;
	NodeRecord rec3;
	NodeRecord rec4;
	NodeRecord rec5;
	NodeRecord rec6;

	
	rec1.setEstimatedCost(1.0);
	rec2.setEstimatedCost(2.0);
	rec3.setEstimatedCost(2.0);
	rec4.setEstimatedCost(1.0);
	rec5.setEstimatedCost(2.0);
	rec6.setEstimatedCost(3.0);

	priorityQueue->insert(rec1);
	priorityQueue->insert(rec2);
	priorityQueue->insert(rec3);
	priorityQueue->insert(rec4);
	priorityQueue->insert(rec5);
	priorityQueue->insert(rec6);

	priorityQueue->deleteMin();
	priorityQueue->deleteMin();
	priorityQueue->deleteMin();
	priorityQueue->deleteMin();
	priorityQueue->deleteMin();
	priorityQueue->deleteMin();

	
	std::cout << "Priority Queue First element: " << priorityQueue->findMin().getCostSoFar() << std::endl;
	priorityQueue->deleteMin();
	std::cout << "Priority Queue First element: " << priorityQueue->findMin().getCostSoFar() << std::endl;
/*
	*/

	//record1->addCost(5.0);
	//record2->addCost(3.2);

	//assert(*record1 > *record2);
	//assert(*record2 < *record1);
}

void AStarPathfinder::testAStar()
{
	AStarSearch* testSearch = new AStarSearch(testGraph_,0,63);
	testSearch->search();

	std::list<uint32> pathToGoal = testSearch->getShortestPath();
	std::list<uint32>::const_iterator pathIt;

	std::cout << "Path to goal: ";
	for(pathIt = pathToGoal.begin(); pathIt!=pathToGoal.end(); ++pathIt)
	{
		std::cout << *pathIt << "->";
	}

	std::cout << std::endl;
	testSearch->printStatistics();

}